/*------------------------------------------------------------------------/
/  Universal string handler for user console interface
/-------------------------------------------------------------------------/
/
/  Copyright (C) 2011, ChaN, all right reserved.
/
/ * This software is a free software and there is NO WARRANTY.
/ * No restriction on use. You can use, modify and redistribute it for
/   personal, non-profit or commercial products UNDER YOUR RESPONSIBILITY.
/ * Redistributions of source code must retain the above copyright notice.
/
/-------------------------------------------------------------------------*/

#include <stdarg.h>

#include "uart.h"
#include "xprintf.h"

/*----------------------------------------------*/
/* Put a character                              */
/*----------------------------------------------*/

void xputc (char c)
{
    if ('\n' == c) {
        uart_send_wait(UART1, (uint8_t)'\r');
    }

    uart_send_wait(UART1, (uint8_t)c);
}


/*----------------------------------------------*/
/* Put a null-terminated string                 */
/*----------------------------------------------*/

void xputs (                    /* Put a string to the default device */
    const char* str             /* Pointer to the string */
)
{
    while (*str)
        xputc(*str++);
}


/*----------------------------------------------*/
/* Formatted string output                      */
/*----------------------------------------------*/
/*  xprintf("%d", 1234);            "1234"
    xprintf("%6d,%3d%%", -200, 5);  "  -200,  5%"
    xprintf("%-6u", 100);           "100   "
    xprintf("%ld", 12345678L);      "12345678"
    xprintf("%04x", 0xA3);          "00a3"
    xprintf("%08LX", 0x123ABC);     "00123ABC"
    xprintf("%016b", 0x550F);       "0101010100001111"
    xprintf("%s", "String");        "String"
    xprintf("%-4s", "abc");         "abc "
    xprintf("%4s", "abc");          " abc"
    xprintf("%c", 'a');             "a"
    xprintf("%f", 10.0);            <xprintf lacks floating point support>
*/

static
void xvprintf (
    const char* fmt,    /* Pointer to the format string */
    va_list arp         /* Pointer to arguments */
)
{
    unsigned int r, i, j, w, f;
    unsigned long v;
    char s[16], c, d, *p;


    for (;;) {
        c = *fmt++;                 /* Get a char */
        if (!c) break;              /* End of format? */
        if (c != '%') {             /* Pass through it if not a % sequense */
            xputc(c); continue;
        }
        f = 0;
        c = *fmt++;                 /* Get first char of the sequense */
        if (c == '0') {             /* Flag: '0' padded */
            f = 1; c = *fmt++;
        } else {
            if (c == '-') {         /* Flag: left justified */
                f = 2; c = *fmt++;
            }
        }
        for (w = 0; c >= '0' && c <= '9'; c = *fmt++)   /* Minimum width */
            w = w * 10 + c - '0';
        if (c == 'l' || c == 'L') { /* Prefix: Size is long int */
            f |= 4; c = *fmt++;
        }
        if (!c) break;              /* End of format? */
        d = c;
        if (d >= 'a') d -= 0x20;
        switch (d) {                /* Type is... */
        case 'S' :                  /* String */
            p = va_arg(arp, char*);
            for (j = 0; p[j]; j++) ;
            while (!(f & 2) && j++ < w) xputc(' ');
            xputs(p);
            while (j++ < w) xputc(' ');
            continue;
        case 'C' :                  /* Character */
            xputc((char)va_arg(arp, int)); continue;
        case 'B' :                  /* Binary */
            r = 2; break;
        case 'O' :                  /* Octal */
            r = 8; break;
        case 'D' :                  /* Signed decimal */
        case 'U' :                  /* Unsigned decimal */
            r = 10; break;
        case 'X' :                  /* Hexdecimal */
            r = 16; break;
        default:                    /* Unknown type (passthrough) */
            xputc(c); continue;
        }

        /* Get an argument and put it in numeral */
        v = (f & 4) ? va_arg(arp, long) : ((d == 'D') ? (long)va_arg(arp, int) : (long)va_arg(arp, unsigned int));
        if (d == 'D' && (v & 0x80000000)) {
            v = 0 - v;
            f |= 8;
        }
        i = 0;
        do {
            if (2 == r) {d = (char)(v & 0x00000001); v >>= 1;}
            else if (8 == r) {d = (char)(v & 0x00000007); v >>= 3;}
            else if (10 == r) {d = (char)(v % r); v /= r;}
            else if (16 == r) {d = (char)(v & 0x0000000f); v >>= 4;}
            else {d = 0; v = 0;}
            if (d > 9) d += (c == 'x') ? 0x27 : 0x07;
            s[i++] = d + '0';
        } while (v && i < sizeof(s));
        if (f & 8) s[i++] = '-';
        j = i; d = (f & 1) ? '0' : ' ';
        while (!(f & 2) && j++ < w) xputc(d);
        do xputc(s[--i]); while(i);
        while (j++ < w) xputc(' ');
    }
}

void xprintf (          /* Put a formatted string to the default device */
    const char* fmt,    /* Pointer to the format string */
    ...                 /* Optional arguments */
)
{
    va_list arp;

#ifdef OS_ENABLE_RT_THREAD
    extern uint32_t rt_hw_interrupt_disable(void);
    extern void rt_hw_interrupt_enable(uint32_t);
#else
    extern uint32_t interrupt_disable(void);
    extern void interrupt_enable(uint32_t);
#endif

#ifdef OS_ENABLE_RT_THREAD
    uint32_t level = rt_hw_interrupt_disable();
#else
    uint32_t level = interrupt_disable();
#endif

    va_start(arp, fmt);
    xvprintf(fmt, arp);
    va_end(arp);

#ifdef OS_ENABLE_RT_THREAD
    rt_hw_interrupt_enable(level);
#else
    interrupt_enable(level);
#endif
}
